package com.pudu.pudusclient;

import android.content.Intent;
import android.util.Log;

import com.pudu.mydemo.view.rplidar_node_t;
import com.pudu.pdrobot.App;

import java.util.ArrayList;
import java.util.List;

/**
 * Created by fanyp on 2016/8/14.
 */
public class RPLidarData {

    static RPLidarData mInstance;

    private RPLidarData()
    {
    }

    public static RPLidarData getInstance(){
        if( mInstance == null)
        {
            mInstance = new  RPLidarData();
        }
        return mInstance;
    }


    public final static int RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT = 2;
    public final static int RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT = 1;
    public final static float PI = (float) 3.14159265;
    public final static float distScale =  (float)0.1;

    public List<rplidar_node_t> ParserData(byte[] newReceiveData)
    {
        int i = 0;
        List<rplidar_node_t> nodelist = new ArrayList<rplidar_node_t>();

        for(i = 0; (i + 5) < newReceiveData.length; i+=5 )
        {
            rplidar_node_t node = new rplidar_node_t();

            node.quality = (byte) ((newReceiveData[i]&0x00ff)>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);

            char myangle = (char)(( (newReceiveData[i+1]&0x00ff)) |  (newReceiveData[i+2]&0x00ff)<< 8 );
            node.angle = (float) ( (myangle>>RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0);

            if( node.angle > 360 || node.angle < 0)
            {
                sendDataParserBroadcast();
                return nodelist;
            }

            char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
            if(mydist == 0)
            {
                continue;
            }
            node.dist = (float)(mydist)/4.0f;

            float distPixel = node.dist;
            float rad = (float) (node.angle * PI / 180.0);
            node.ptX = (float) (Math.sin(rad) * (distPixel));
            node.ptY = (float) (Math.cos(rad) * (distPixel));
            Log.i("aaaa", "node.quality:" + node.quality
                    + "|node.angle:" + node.angle
                    + "|node.dist:" + node.dist
                    + "|node.ptX:" + node.ptX
                    + "|node.ptY:" + node.ptY);
            nodelist.add(node);
        }

        return nodelist;
    }



    public List<rplidar_node_t> ParserData2PC(byte[] newReceiveData)
    {
        int i = 0;
        List<rplidar_node_t> nodelist = new ArrayList<rplidar_node_t>();

        for(i = 0; (i + 5) < newReceiveData.length; i+=5 )
        {
            rplidar_node_t node = new rplidar_node_t();

            node.quality = (byte) ((newReceiveData[i]&0x00ff)>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);

            if( node.angle > 360 || node.angle < 0)
            {
                sendDataParserBroadcast();
                return nodelist;
            }

            char myangle = (char)(( (newReceiveData[i+1]&0x00ff)) |  (newReceiveData[i+2]&0x00ff)<< 8 );
            node.angle = (float) ( (myangle>>RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0);

            char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
            if(mydist == 0)
            {
                continue;
            }

            if( node.angle <= 270 && node.angle >= 90 )
            {
                continue;
            }


            node.dist = (float)(mydist)/4.0f;

            float distPixel = node.dist;
            float rad = (float) (node.angle * PI / 180.0);
            node.ptX = (float) (Math.sin(rad) * (distPixel));
            node.ptY = (float) (Math.cos(rad) * (distPixel));
            Log.i("aaaa", "node.quality:" + node.quality
                    + "|node.angle:" + node.angle
                    + "|node.dist:" + node.dist
                    + "|node.ptX:" + node.ptX
                    + "|node.ptY:" + node.ptY);
            nodelist.add(node);
        }

        return nodelist;
    }

    public static int Invalid_Dist = -1;

    //六个角度扇形区域内最近的障碍物距离
    public static float DistObstacle_0_a = Invalid_Dist;
    public static float DistObstacle_0_b = Invalid_Dist;
    public static float DistObstacle_0_c = Invalid_Dist;

    public static float DistObstacle_1_a = Invalid_Dist;
    public static float DistObstacle_1_b = Invalid_Dist;
    public static float DistObstacle_1_c = Invalid_Dist;

    public static float DistObstacle_2_a = Invalid_Dist;
    public static float DistObstacle_2_b = Invalid_Dist;
    public static float DistObstacle_2_c = Invalid_Dist;

    public static float DistObstacle_3_a = Invalid_Dist;
    public static float DistObstacle_3_b = Invalid_Dist;
    public static float DistObstacle_3_c = Invalid_Dist;

    public static float DistObstacle_4_a = Invalid_Dist;
    public static float DistObstacle_4_b = Invalid_Dist;
    public static float DistObstacle_4_c = Invalid_Dist;

    public static float DistObstacle_5_a = Invalid_Dist;
    public static float DistObstacle_5_b = Invalid_Dist;
    public static float DistObstacle_5_c = Invalid_Dist;

    //初始化6个角度的数据
    public static int ANGLE_0_MIN = 270;
    public static int ANGLE_0_MAX = 300;
    public static int ANGLE_1_MIN = 300;
    public static int ANGLE_1_MAX = 330;
    public static int ANGLE_2_MIN = 330;
    public static int ANGLE_2_MAX = 360;
    public static int ANGLE_3_MIN = 0;
    public static int ANGLE_3_MAX = 30;
    public static int ANGLE_4_MIN = 30;
    public static int ANGLE_4_MAX = 60;
    public static int ANGLE_5_MIN = 60;
    public static int ANGLE_5_MAX = 90;

  /*  public List<rplidar_node_t> ParserDataOnWayDis(byte[] newReceiveData) {
        DistObstacle_0 = DistDetectMax;
        DistObstacle_1 = DistDetectMax;
        DistObstacle_2 = DistDetectMax;
        DistObstacle_3 = DistDetectMax;
        DistObstacle_4 = DistDetectMax;
        DistObstacle_5 = DistDetectMax;

        List<rplidar_node_t> nodelist = new ArrayList<rplidar_node_t>();

        int i = 0;

        for(i = 0; (i + 5) < newReceiveData.length; i+=5 )
        {
            rplidar_node_t node = new rplidar_node_t();

            node.quality = (byte) ((newReceiveData[i]&0x00ff)>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);

            char myangle = (char)(( (newReceiveData[i+1]&0x00ff)) |  (newReceiveData[i+2]&0x00ff)<< 8 );
            node.angle = (float) ( (myangle>>RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0);

            if(ANGLE_0_MIN <= node.angle && ANGLE_0_MAX > node.angle) {
                char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
                if(mydist == 0)
                {
                    continue;
                }
                node.dist = (float)(mydist)/4.0f;
                if( node.dist < DistObstacle_0)
                {
                    DistObstacle_0 = node.dist;
                }

                float distPixel = node.dist * distScale;
                float rad = (float) (node.angle * PI / 180.0);
                node.ptX = (float) (Math.sin(rad) * (distPixel));
                node.ptY = (float) (Math.cos(rad) * (distPixel));

                nodelist.add(node);
            }else if(ANGLE_1_MIN <= node.angle && ANGLE_1_MAX > node.angle){
                char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
                if(mydist == 0)
                {
                    continue;
                }
                node.dist = (float)(mydist)/4.0f;
                if( node.dist < DistObstacle_1)
                {
                    DistObstacle_1 = node.dist;
                }

                float distPixel = node.dist * distScale;
                float rad = (float) (node.angle * PI / 180.0);
                node.ptX = (float) (Math.sin(rad) * (distPixel));
                node.ptY = (float) (Math.cos(rad) * (distPixel));

                nodelist.add(node);
            }else if(ANGLE_2_MIN <= node.angle && ANGLE_2_MAX > node.angle){
                char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
                if(mydist == 0)
                {
                    continue;
                }
                node.dist = (float)(mydist)/4.0f;
                if( node.dist < DistObstacle_2)
                {
                    DistObstacle_2 = node.dist;
                }
                float distPixel = node.dist * distScale;
                float rad = (float) (node.angle * PI / 180.0);
                node.ptX = (float) (Math.sin(rad) * (distPixel));
                node.ptY = (float) (Math.cos(rad) * (distPixel));

                nodelist.add(node);

            }else if(ANGLE_3_MIN <= node.angle && ANGLE_3_MAX > node.angle){
                char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
                if(mydist == 0)
                {
                    continue;
                }
                node.dist = (float)(mydist)/4.0f;
                if( node.dist < DistObstacle_3)
                {
                    DistObstacle_3 = node.dist;
                }
                float distPixel = node.dist * distScale;
                float rad = (float) (node.angle * PI / 180.0);
                node.ptX = (float) (Math.sin(rad) * (distPixel));
                node.ptY = (float) (Math.cos(rad) * (distPixel));

                nodelist.add(node);
            }else if(ANGLE_4_MIN <= node.angle && ANGLE_4_MAX > node.angle){
                char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
                if(mydist == 0)
                {
                    continue;
                }
                node.dist = (float)(mydist)/4.0f;
                if( node.dist < DistObstacle_4)
                {
                    DistObstacle_4 = node.dist;
                }
                float distPixel = node.dist * distScale;
                float rad = (float) (node.angle * PI / 180.0);
                node.ptX = (float) (Math.sin(rad) * (distPixel));
                node.ptY = (float) (Math.cos(rad) * (distPixel));

                nodelist.add(node);
            }else if(ANGLE_5_MIN <= node.angle && ANGLE_5_MAX > node.angle) {
                char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
                if(mydist == 0)
                {
                    continue;
                }
                node.dist = (float)(mydist)/4.0f;
                if( node.dist < DistObstacle_5)
                {
                    DistObstacle_5 = node.dist;
                }
                float distPixel = node.dist * distScale;
                float rad = (float) (node.angle * PI / 180.0);
                node.ptX = (float) (Math.sin(rad) * (distPixel));
                node.ptY = (float) (Math.cos(rad) * (distPixel));

                nodelist.add(node);
            }else {
                continue;
            }
        }

        Log.i("bbbb", "Angle "+ ANGLE_0_MIN + "-" + ANGLE_0_MAX + ":" + DistObstacle_0);
        Log.i("bbbb", "Angle "+ ANGLE_1_MIN + "-" + ANGLE_1_MAX + ":" + DistObstacle_1);
        Log.i("bbbb", "Angle "+ ANGLE_2_MIN + "-" + ANGLE_2_MAX + ":" + DistObstacle_2);
        Log.i("bbbb", "Angle "+ ANGLE_3_MIN + "-" + ANGLE_3_MAX + ":" + DistObstacle_3);
        Log.i("bbbb", "Angle "+ ANGLE_4_MIN + "-" + ANGLE_4_MAX + ":" + DistObstacle_4);
        Log.i("bbbb", "Angle "+ ANGLE_5_MIN + "-" + ANGLE_5_MAX + ":" + DistObstacle_5);

        return nodelist;
    }*/


    public void sendDataParserBroadcast()
    {
        try {
            Intent intent = new Intent();
            intent.setAction(RadarUsbService.ACTION_USB_DATAERROR_RESET);
            App.getInstance().sendBroadcast(intent);
        }catch(Exception e)
        {
            e.printStackTrace();
        }
    }

    public void ParserFrontScanData(byte[] newReceiveData) {

        int i = 0;
        int nOutofRange = 10;

        for(i = 0; (i + 5) < newReceiveData.length; i+=5 )
        {
            rplidar_node_t node = new rplidar_node_t();

            node.quality = (byte) ((newReceiveData[i]&0x00ff)>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);

            char myangle = (char)(( (newReceiveData[i+1]&0x00ff)) |  (newReceiveData[i+2]&0x00ff)<< 8 );
            node.angle = (float) ( (myangle>>RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0);

            if(ANGLE_0_MIN <= node.angle && ANGLE_0_MAX > node.angle) {

                //开始采样，进入采样周期
                nOutofRange = 0;
                char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
                if(mydist == 0)
                {
                    continue;
                }

                float dist = (float)(mydist)/4.0f;
                if( DistObstacle_0_b == Invalid_Dist){
                    DistObstacle_0_b = dist;
                    continue;
                }
                if( DistObstacle_0_a == Invalid_Dist){
                    if(dist < DistObstacle_0_b) {
                        DistObstacle_0_a = dist;
                    }else{
                        DistObstacle_0_a = DistObstacle_0_b;
                        DistObstacle_0_b = dist;
                    }
                    continue;
                }

                if( dist < DistObstacle_0_a ) {
                    DistObstacle_0_b = DistObstacle_0_a;
                    DistObstacle_0_a = dist;
                }else if(dist < DistObstacle_0_b) {
                    DistObstacle_0_b = dist;
                }

            }else if(ANGLE_1_MIN <= node.angle && ANGLE_1_MAX > node.angle){
                char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
                if(mydist == 0)
                {
                    continue;
                }

                float dist = (float)(mydist)/4.0f;
                if( DistObstacle_1_b == Invalid_Dist){
                    DistObstacle_1_b = dist;
                    continue;
                }
                if( DistObstacle_1_a == Invalid_Dist){
                    if(dist < DistObstacle_1_b) {
                        DistObstacle_1_a = dist;
                    }else{
                        DistObstacle_1_a = DistObstacle_1_b;
                        DistObstacle_1_b = dist;
                    }
                    continue;
                }

                if( dist < DistObstacle_1_a ) {
                    DistObstacle_1_b = DistObstacle_1_a;
                    DistObstacle_1_a = dist;
                }else if(dist < DistObstacle_1_b) {
                    DistObstacle_1_b = dist;
                }
            }else if(ANGLE_2_MIN <= node.angle && ANGLE_2_MAX > node.angle){
                char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
                if(mydist == 0)
                {
                    continue;
                }

                float dist = (float)(mydist)/4.0f;
                if( DistObstacle_2_b == Invalid_Dist){
                    DistObstacle_2_b = dist;
                    continue;
                }
                if( DistObstacle_2_a == Invalid_Dist){
                    if(dist < DistObstacle_2_b) {
                        DistObstacle_2_a = dist;
                    }else{
                        DistObstacle_2_a = DistObstacle_2_b;
                        DistObstacle_2_b = dist;
                    }
                    continue;
                }

                if( dist < DistObstacle_2_a ) {
                    DistObstacle_2_b = DistObstacle_2_a;
                    DistObstacle_2_a = dist;
                }else if(dist < DistObstacle_2_b) {
                    DistObstacle_2_b = dist;
                }
            }else if(ANGLE_3_MIN <= node.angle && ANGLE_3_MAX > node.angle){
                char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
                if(mydist == 0)
                {
                    continue;
                }

                float dist = (float)(mydist)/4.0f;
                if( DistObstacle_3_b == Invalid_Dist){
                    DistObstacle_3_b = dist;
                    continue;
                }
                if( DistObstacle_3_a == Invalid_Dist){
                    if(dist < DistObstacle_3_b) {
                        DistObstacle_3_a = dist;
                    }else{
                        DistObstacle_3_a = DistObstacle_3_b;
                        DistObstacle_3_b = dist;
                    }
                    continue;
                }

                if( dist < DistObstacle_3_a ) {
                    DistObstacle_3_b = DistObstacle_3_a;
                    DistObstacle_3_a = dist;
                }else if(dist < DistObstacle_3_b) {
                    DistObstacle_3_b = dist;
                }
            }else if(ANGLE_4_MIN <= node.angle && ANGLE_4_MAX > node.angle){
                char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
                if(mydist == 0)
                {
                    continue;
                }

                float dist = (float)(mydist)/4.0f;
                if( DistObstacle_4_b == Invalid_Dist){
                    DistObstacle_4_b = dist;
                    continue;
                }
                if( DistObstacle_4_a == Invalid_Dist){
                    if(dist < DistObstacle_4_b) {
                        DistObstacle_4_a = dist;
                    }else{
                        DistObstacle_4_a = DistObstacle_4_b;
                        DistObstacle_4_b = dist;
                    }
                    continue;
                }

                if( dist < DistObstacle_4_a ) {
                    DistObstacle_4_b = DistObstacle_4_a;
                    DistObstacle_4_a = dist;
                }else if(dist < DistObstacle_4_b) {
                    DistObstacle_4_b = dist;
                }
            }else if(ANGLE_5_MIN <= node.angle && ANGLE_5_MAX > node.angle) {
                char mydist = (char)(( (newReceiveData[i+3]&0x00ff) ) |  (newReceiveData[i+4]&0x00ff)<< 8 );
                if(mydist == 0)
                {
                    continue;
                }

                float dist = (float)(mydist)/4.0f;
                if( DistObstacle_5_b == Invalid_Dist){
                    DistObstacle_5_b = dist;
                    continue;
                }
                if( DistObstacle_5_a == Invalid_Dist){
                    if(dist < DistObstacle_5_b) {
                        DistObstacle_5_a = dist;
                    }else{
                        DistObstacle_5_a = DistObstacle_5_b;
                        DistObstacle_5_b = dist;
                    }
                    continue;
                }

                if( dist < DistObstacle_5_a ) {
                    DistObstacle_5_b = DistObstacle_5_a;
                    DistObstacle_5_a = dist;
                }else if(dist < DistObstacle_5_b) {
                    DistObstacle_5_b = dist;
                }
            }else {
                nOutofRange ++;
                //遇到超过10个非法范围退出采样
                //保留当前数据到c频道
                if( nOutofRange >= 10)
                {
                    if(DistObstacle_0_b!= Invalid_Dist) {
                        DistObstacle_0_c = DistObstacle_0_b;
                        DistObstacle_0_b = DistObstacle_0_a = Invalid_Dist;
                    }
                    if(DistObstacle_1_b!= Invalid_Dist) {
                        DistObstacle_1_c = DistObstacle_1_b;
                        DistObstacle_1_b = DistObstacle_1_a = Invalid_Dist;
                    }
                    if(DistObstacle_2_b!= Invalid_Dist) {
                        DistObstacle_2_c = DistObstacle_2_b;
                        DistObstacle_2_b = DistObstacle_2_a = Invalid_Dist;
                    }
                    if(DistObstacle_3_b!= Invalid_Dist) {
                        DistObstacle_3_c = DistObstacle_3_b;
                        DistObstacle_3_b = DistObstacle_3_a = Invalid_Dist;
                    }
                    if(DistObstacle_4_b!= Invalid_Dist) {
                        DistObstacle_4_c = DistObstacle_4_b;
                        DistObstacle_4_b = DistObstacle_4_a = Invalid_Dist;
                    }
                    if(DistObstacle_5_b!= Invalid_Dist) {
                        DistObstacle_5_c = DistObstacle_5_b;
                        DistObstacle_5_b = DistObstacle_5_a = Invalid_Dist;
                    }
                }
                continue;
            }
        }

        nOutofRange = 0;

        if(DistObstacle_0_c < 500)
        {
            Log.i("bbbb", "Angle "+ ANGLE_0_MIN + "-" + ANGLE_0_MAX + ":" + DistObstacle_0_c);
        }
        if(DistObstacle_1_c < 500)
        {
            Log.i("bbbb", "Angle "+ ANGLE_1_MIN + "-" + ANGLE_1_MAX + ":" + DistObstacle_1_c);
        }

        if(DistObstacle_2_c < 500)
        {
            Log.i("bbbb", "Angle "+ ANGLE_2_MIN + "-" + ANGLE_2_MAX + ":" + DistObstacle_2_c);
        }

        if(DistObstacle_3_c < 500)
        {
            Log.i("bbbb", "Angle "+ ANGLE_3_MIN + "-" + ANGLE_3_MAX + ":" + DistObstacle_3_c);
        }

        if(DistObstacle_4_c < 500)
        {
            Log.i("bbbb", "Angle "+ ANGLE_4_MIN + "-" + ANGLE_4_MAX + ":" + DistObstacle_4_c);
        }

        if(DistObstacle_5_c < 500)
        {
            Log.i("bbbb", "Angle "+ ANGLE_5_MIN + "-" + ANGLE_5_MAX + ":" + DistObstacle_5_c);
        }
    }

    public static int Distance_In_Front_Obstacle = 0;
    //最小的两个值 a--最小， b -- 次小
    private int Distance_In_Front_Obstacle_inner_a = -1;
    private int Distance_In_Front_Obstacle_inner_b = -1;
    int nMyOutofRange = 0;
    public void ParserFrontScanDataV2(byte[] newReceiveData){
        int i = 0;

        Distance_In_Front_Obstacle_inner_a = -1;
        Distance_In_Front_Obstacle_inner_b = -1;

        if( Distance_In_Front_Obstacle == -1 || Distance_In_Front_Obstacle > 500 )
        {
            Log.i("bnbnbnb", "nothing");
        }
        else
        {
            Log.i("bnbnbnb", "something");
        }

        for(i = 0; (i + 5) < newReceiveData.length; i+=5 ) {

            rplidar_node_t node = new rplidar_node_t();

            node.quality = (byte) ((newReceiveData[i]&0x00ff)>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);

            char myangle = (char)(( (newReceiveData[i+1]&0x00ff)) |  (newReceiveData[i+2]&0x00ff)<< 8 );
            node.angle = (float) ( (myangle>>RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0);

            if( node.angle > 360 || node.angle < 0)
            {
                sendDataParserBroadcast();
                return;
            }

            Log.i("radaraaa", "aaaaaaaaaaaa:" + node.angle );

            if((270 <= node.angle && 360 > node.angle) ||
                    (0 <= node.angle && 90 > node.angle) ) {

                nMyOutofRange = 0;

                char mydist = (char) (((newReceiveData[i + 3] & 0x00ff)) | (newReceiveData[i + 4] & 0x00ff) << 8);
                if (mydist == 0) {
                    Log.i("radaraaa", "aaaaaaaaaaaa:" + node.angle + "node.ptX:" + "0000000" );
                    continue;
                }
                node.dist = (float) (mydist) / 4.0f;

                float distPixel = node.dist;
                float rad = (float) (node.angle * PI / 180.0);
                node.ptX = (float) (Math.sin(rad) * (distPixel));
                node.ptY = (float) (Math.cos(rad) * (distPixel));

                Log.i("radaraaa", "aaaaaaaaaaaa:" + node.angle + "node.ptX:" + node.ptX );

                //Log.i("radaraaa", "node.ptX:" + node.ptX + "--node.ptY:" + node.ptY);
                if(Math.abs(node.ptX) <= 310)
                {
                    Log.i("radaraaa", "aaaaaaaaaaaa" );
                    if( Distance_In_Front_Obstacle_inner_b == -1)
                    {
                        Distance_In_Front_Obstacle_inner_b = (int) Math.abs(node.dist);
                        continue;
                    }

                    if(Distance_In_Front_Obstacle_inner_a == -1)
                    {
                        if(Distance_In_Front_Obstacle_inner_b > node.dist) {
                            Distance_In_Front_Obstacle_inner_a = (int) node.dist;
                        }else{
                            Distance_In_Front_Obstacle_inner_a = Distance_In_Front_Obstacle_inner_b;
                            Distance_In_Front_Obstacle_inner_b = (int) node.dist;
                        }
                        continue;
                    }

                    if( Distance_In_Front_Obstacle_inner_b > node.dist
                            && Distance_In_Front_Obstacle_inner_a < node.dist)
                    {
                        Distance_In_Front_Obstacle_inner_b = (int)node.dist;
                    }
                    else if( Distance_In_Front_Obstacle_inner_a > node.dist)
                    {
                        Distance_In_Front_Obstacle_inner_b = Distance_In_Front_Obstacle_inner_a;
                        Distance_In_Front_Obstacle_inner_a = (int)node.dist;
                    }
                    else if( Distance_In_Front_Obstacle_inner_b < node.dist){
                        continue;
                    }
                    continue;
                }
            }else {
                if( nMyOutofRange <= 10) {
                    nMyOutofRange++;

                    if (nMyOutofRange > 10) {

                        //使用次近距离的点
                        Distance_In_Front_Obstacle = Distance_In_Front_Obstacle_inner_b;

                        Log.i("radaraaa", "Distance_In_Front_Obstacle:" + Distance_In_Front_Obstacle);

                        Distance_In_Front_Obstacle_inner_b = -1;
                        Distance_In_Front_Obstacle_inner_a = -1;
                    }
                    continue;
                }
            }

        }
    }

}
